#ifndef __SG_PCL_DENOISE_H__
#define __SG_PCL_DENOISE_H__

#include "SGPCLJsonConfig.h"

#include <pcl/filters/statistical_outlier_removal.h>

namespace sgpcl
{

/// <summary>
/// 去噪算法参数(基于平均距离）
/// </summary>
struct SGDeNoiseConfig : public SGPCLJsonConfig
{
  int nNeigbourNum;       // 使用多少个临近点计算平均距离
  float fStddevMulThresh; // 多少个标准差之外的点视为噪点

  SGPCL_API SGDeNoiseConfig();

  SGPCL_API ~SGDeNoiseConfig() = default;

  /// <summary>
  /// 解析Json字符串，读取参数
  /// </summary>
  /// <param name="sJsonRoot">Json节点</param>
  SGPCL_API void ParseJson(const rapidjson::Value& sJsonRoot) final override;
};

template<typename PointT>
double ComputeCloudResolution(typename pcl::PointCloud<PointT>::ConstPtr cloud)
{
  double res = 0.0;
  int n_points = 0;
  int nres;
  std::vector<int> indices(2);
  std::vector<float> sqr_distances(2);
  // step1: 新建kdtree用于搜索
  pcl::search::KdTree<PointT> tree;
  tree.setInputCloud(cloud);

  // step2: 遍历点云每个点，并找出与它距离最近的点
  for (size_t i = 0; i < cloud->size(); ++i)
  {
    if (!std::isfinite((*cloud)[i].x))
    {
      continue;
    }
    // 取第二个距离，因为第一个是它本身
    nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
    // step3: 统计最小距离和、有效点数量
    if (nres == 2)
    {
      res += sqrt(sqr_distances[1]);
      ++n_points;
    }
  }

  // step4: 计算空间分辨率
  if (n_points != 0)
  {
    res /= n_points;
  }
  return res;
}


/// <summary>
/// 对带有坐标信息的点云去噪（基于平均临近距离）
/// </summary>
/// <typeparam name="PointT">对带有坐标信息的点类型</typeparam>
/// <param name="spCloud">原点云</param>
/// <param name="sConfig">算法参数</param>
/// <returns>去噪后的点云</returns>
template <typename PointT>
typename pcl::PointCloud<PointT>::Ptr DeNoise(
  typename pcl::PointCloud<PointT>::ConstPtr spCloud, const SGDeNoiseConfig& sConfig,
  float& fResolution)
{
  // 计时器
  SG_TIMER("去噪");

  if (sConfig.fStddevMulThresh <= 0) // 标准差倍数必须为正数
    SG_PANIC("标准差倍数必须为正数！");

  // 生成滤波对象
  pcl::StatisticalOutlierRemoval<PointT> sor;
  // 导入点云
  sor.setInputCloud(spCloud);
  // 使用多少个临近点计算平均距离
  sor.setMeanK(sConfig.nNeigbourNum);
  // 将多少个标准差之外的点视为噪点
  sor.setStddevMulThresh(sConfig.fStddevMulThresh);
  // 进行过滤
  pcl::PointCloud<PointT>::Ptr spDenoised(new pcl::PointCloud<PointT>);
  sor.filter(*spDenoised);

  // 计算点云分辨率
  fResolution = ComputeCloudResolution<PointT>(spDenoised);

  return spDenoised;
}

} // namespace sgtunnelface

#endif // !__SG_PCL_DENOISE_H__
